#include "stm32f10x.h"
#include "pwm.h"
#include "delay.h"
#include "motor.h"


void motorSet(u8 set, u8 enable){
		switch (set)
		{
		case 0:
			motorControl(0,0,0,0,enable);
			delay_ms(20);
			motorControl(0,0,0,0,enable);
			break;

		case 1:
			motorControl(0,0,0,0,enable);
			delay_ms(20);
			motorControl(1,0,0,1,enable);
			
			break;

		case 2:
			motorControl(0,0,0,0,enable);
			delay_ms(20);
			motorControl(0,1,1,0,enable);
			
			break;

		case 3:
			motorControl(0,0,0,0,enable);
			delay_ms(20);
			motorControl(1,0,1,0,enable);
			
			break;
		
		case 4:
			motorControl(0,0,0,0,enable);
			delay_ms(20);
			motorControl(0,1,0,1,enable);
			break;

		default:
			break;
		}
}


void motorControl(float speed1, float speed2, float speed3, float speed4,u8 enable){
	int PWM1 = 0, PWM2 = 0, PWM3 = 0, PWM4 = 0;
	if(enable){
		PWM1 = speed1 * 999;
		PWM2 = speed2 * 999;
		PWM3 = speed3 * 999;
		PWM4 = speed4 * 999;
		TIM3_PWM_update(PWM1,PWM2,PWM3,PWM4);
	}
}


